#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "common_lib/glog/glog.h"
#include <opencv2/opencv.hpp>
#include "ceres/ceres.h"

// 参考 《视觉SLAM十四讲》第二版 6.3.2 章节
// ros2 run common_test test_fitting_curve_ceres

// xxty@xxty-virtual-machine:~/trobot/ros2_ws$ ros2 run common_test test_fitting_curve_ceres
// iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
//    0  1.597873e+06    0.00e+00    3.52e+06   0.00e+00   0.00e+00  1.00e+04        0    7.61e-05    2.07e-02
//    1  1.884440e+05    1.41e+06    4.86e+05   9.88e-01   8.82e-01  1.81e+04        1    6.04e-04    2.80e-02
//    2  1.784821e+04    1.71e+05    6.78e+04   9.89e-01   9.06e-01  3.87e+04        1    4.98e-05    2.81e-02
//    3  1.099631e+03    1.67e+04    8.58e+03   1.10e+00   9.41e-01  1.16e+05        1    4.41e-05    2.81e-02
//    4  8.784938e+01    1.01e+03    6.53e+02   1.51e+00   9.67e-01  3.48e+05        1    5.41e-05    2.82e-02
//    5  5.141230e+01    3.64e+01    2.72e+01   1.13e+00   9.90e-01  1.05e+06        1    4.32e-05    2.83e-02
//    6  5.096862e+01    4.44e-01    4.27e-01   1.89e-01   9.98e-01  3.14e+06        1    4.32e-05    2.83e-02
//    7  5.096851e+01    1.10e-04    9.53e-04   2.84e-03   9.99e-01  9.41e+06        1    4.29e-05    2.84e-02
// solve time cost = 0.0326175 seconds. 
// Ceres Solver Report: Iterations: 8, Initial cost: 1.597873e+06, Final cost: 5.096851e+01, Termination: CONVERGENCE
// estimated a=0.890908 b=2.1719 c=0.943628

using namespace std;

// 代价函数计算模型
struct CURVE_FITTING_COST{
    CURVE_FITTING_COST(double x, double y):_x(x), _y(y){}

    // 残差的计算模型
    template<typename T>
    bool operator()(const T * const abc, T * residual) const{
        // y-exp(ax^2+bx+c) 的计算模型
        residual[0] = T(_y) - ceres::exp(abc[0]*T(_x)*T(_x) + abc[1]*T(_x) + abc[2]);
        return true;
    }

    const double _x, _y;
};

class FittingCurveCeres : public rclcpp::Node{
    public:
        FittingCurveCeres();
        ~FittingCurveCeres() = default;

        // 拟合曲线
        void FittingCurve();
};


FittingCurveCeres::FittingCurveCeres(): Node("test_fitting_curve_ceres"){
    FittingCurve();
}

void FittingCurveCeres::FittingCurve(){
    double ar=1.0, br=2.0, cr=1.0;      //真实参数值
    double ae=2.0, be=-1.0, ce=5.0;     //估计参数值
    int N=100;  //数据点个数
    double w_sigma=1.0;  //噪声Sigma值
    // double inv_sigma=1.0/w_sigma;
    cv::RNG rng;        //opencv 随机数产生器

    // 构造数据集
    vector<double> x_data, y_data;
    for(int i=0; i<N; i++){
        double x = i/100.0;
        x_data.push_back(x);
        y_data.push_back(exp(ar*x*x + br*x + cr) + rng.gaussian(w_sigma*w_sigma));
    }
    
    // 构建最小二乘问题
    double abc[3] = {ae, be, ce};
    ceres::Problem problem;
    for(int i=0; i<N; i++){
        // 使用自动求导
        ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(
            new CURVE_FITTING_COST(x_data[i], y_data[i]));
        // 向问题添加误差项
        problem.AddResidualBlock(
            cost_function,  // 
            NULL,   //核函数，暂时不用
            abc     //待估计参数
        );   
    }

    // 配置求解器
    ceres::Solver::Options options; //配置项
    options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
    options.minimizer_progress_to_stdout = true;    //输出到stdout

    ceres::Solver::Summary summary; //优化信息
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    ceres::Solve(options, &problem, &summary);  //开始优化
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2-t1);
    cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

    cout << summary.BriefReport() << endl;
    cout << "estimated a=" << abc[0] << " b=" << abc[1] << " c=" << abc[2] << endl;
}


int main(int argc, char * argv[]){
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<FittingCurveCeres>());
    rclcpp::shutdown();
    return 0;
}